/*
 * AngleSMC.cpp
 *
 *  Created on: 31.01.2013
 *      Author: Aleix
 */

#include "rodos.h"
#include "AngleSMC.h"
#include "../Threads/global.h"
#include "../hardware.h"

#include <stdlib.h>
#include "../Threads/global.h"

extern Systemstate state;
extern PowerDriver motor;

AngleSMC::AngleSMC() {
	pre_error_a = 0;
	integral_a = 0;
	output_a = 0;
	S = 0;
	I = 0;
	D = 0;
	state.Gain2ang = 0.21;
	state.Gain3ang = 0;
	state.Gain4ang = 0;
}

AngleSMC::~AngleSMC() {
	// TODO Auto-generated destructor stub
}

void AngleSMC::resetSMC(){
	pre_error_a = 0;
	integral_a = 0;
	output_a = 0;
	S = 0;
	I = 0;
	D = 0;
}

void AngleSMC::run() {

	float error = 0;
	float derivative = 0;

	error = state.desAngle - state.angleKalman;
	if (error > 180) {
		error = error - 360;
	} else if(error < -180){
		error = error + 360;
	}


	// PROPORTIONAL PART (sign)
	if (error <= 0){
		S = -1;
	} else {
		S = 1;
	}
	
	// INTEGRAL PART
	if (abs(error) > state.eps_a) {
		integral_a += error * SAMPLETIME_S;
	}
	
	//TODO Derivative of the desired angle!!
	// DERIVATIVE PART
	// derivative = (desiredAngle - previousDesiredAngle) / SAMPLETIME_S;

	S = state.Gain2ang * S;
	I = state.Gain3ang * integral_a;
	D = state.Gain4ang * derivative;
	output_a = S + I + D;
	xprintf("%f",output_a);

	// Save the error
	pre_error_a = error;

	if (output_a > LIMIT_VOLTAGE) {
		output_a = LIMIT_VOLTAGE;
	} else if (output_a < -LIMIT_VOLTAGE) {
		output_a = -LIMIT_VOLTAGE;
	}

	state.dutycycle = (float)(output_a / MAX_VOLTAGE * 100);
	motor.setSpeedPercentage(state.dutycycle );
}
